// kate: space-indent on; indent-width 8;


.function float2fix(num) {
        .var strValue = ""+toIntString(num*128)
        .return strValue.asNumber()

}

.pc =$0801 "Basic Upstart Program"
:BasicUpstart($0810)

.pc = $0810 "Main Program"
        
        ldx #1
        
        ldy #7
        lda #0
sl:     sta 2040,y
        lda #1
        sta $d027,y
        dey
        bpl sl
        lda #$ff
        sta $D015
ml:     ldy #0
        sty bits9
loop:   tya
        asl
        tax
        
upx:    clc
        lda coordsX,x
        adc vx,x
        sta coordsX,x
        lda coordsX+1,x
        adc vx+1,x
        sta coordsX+1,x
        
        lda coordsX+1,x
        cmp #>float2fix(320)
        bne c1
        lda coordsX,x
        cmp #<float2fix(320)
c1:     bcc upy
        // negate vx
        sec
        lda #0
        sbc vx,x
        sta vx,x
        lda #0
        sbc vx+1,x
        sta vx+1,x
        jmp upx

        
upy:    clc
        lda coordsY,x
        adc vy,x
        sta coordsY,x
        lda coordsY+1,x
        adc vy+1,x
        sta coordsY+1,x
        
        lda coordsY+1,x
        cmp #>float2fix(200)
        bne c2
        lda coordsY,x
        cmp #<float2fix(200)
c2:     bcc ok1
        //negate vy
        sec
        lda #0
        sbc vy,x
        sta vy,x
        lda #0
        sbc vy+1,x
        sta vy+1,x
        jmp upy
        
ok1:    
// Ok, we have our coordinates in fixed point 9/7
// we need to shift 7 places to the right to get rid of fractional
// also we have to correct for spriteX=X+24 and
// spriteY=Y+50

        lda coordsX+1,x
        sta spriteX+1,x
        lda coordsX,x
        sta spriteX,x
        
        //7 shifts right == x/128
        lsr spriteX+1,x
        ror spriteX,x
        lsr spriteX+1,x
        ror spriteX,x
        lsr spriteX+1,x
        ror spriteX,x
        lsr spriteX+1,x
        ror spriteX,x
        lsr spriteX+1,x
        ror spriteX,x
        lsr spriteX+1,x
        ror spriteX,x
        lsr spriteX+1,x
        ror spriteX,x
        clc
        lda spriteX,x
        adc #24
        sta spriteX,x
        lda spriteX+1,x
        adc #0
        sta spriteX+1,x
        


        lda coordsY+1,x
        sta spriteY+1,x
        lda coordsY,x
        sta spriteY,x
        
        //7 shifts right == y/128
        lsr spriteY+1,x
        ror spriteY,x
        lsr spriteY+1,x
        ror spriteY,x
        lsr spriteY+1,x
        ror spriteY,x
        lsr spriteY+1,x
        ror spriteY,x
        lsr spriteY+1,x
        ror spriteY,x
        lsr spriteY+1,x
        ror spriteY,x
        lsr spriteY+1,x
        ror spriteY,x
        clc
        lda spriteY,x
        adc #50
        sta spriteY,x
        lda spriteY+1,x
        adc #0
        sta spriteY+1,x

        lda spriteX+1,x
        lsr
        ror bits9
        lda spriteX,x
        sta $D000,x
        lda spriteY,x
        sta $D001,x

        iny
        cpy #8
        beq done
        jmp loop
done:   lda bits9
        sta $D010
        
        lda #0
        sta $d020  
        
        lda #$80
wait:   cmp $d012
        bne wait
        lda #2
        sta $d020
        
        jsr mul16
        
        lda #3
        sta $d020
        
        jmp ml

        
mul16:  {
        lda vx+1
        bpl nos
        // vx is negative negate
        
        sec
        lda #0
        sbc vx
        sta vx
        lda #0
        sbc vx+1
        sta vx+1
        lda #1
        jmp stor
nos:    lda #0
stor:   sta wasNegative
        
        
        lda #0
        sta product+2 // clear upper bits of product
        sta product+3 
        ldx #$10 // set binary count to 16 
shift_r: lsr vx+1 // divide multiplier by 2 
        ror vx
        bcc rotate_r 
        lda product+2 // get upper half of product and add multiplicand
        clc
        adc acc
        sta product+2
        lda product+3 
        adc acc+1
rotate_r: ror // rotate partial product 
        sta product+3 
        ror product+2
        ror product+1 
        ror product 
        dex
        bne shift_r
        
        
        ldx #7
sh7:    lsr product+3
        ror product+2
        ror product+1
        ror product
        dex
        bne sh7
        
        lda product+1
        sta vx+1
        lda product+0
        sta vx
        
        lda wasNegative
        beq done
        sec
        lda #0
        sbc vx
        sta vx
        lda #0
        sbc vx+1
        sta vx+1

done:   rts
        }
        
        //.print float2fix(3.141259)/128


.pc = 49152
bits9:   .byte 0
wasNegative: .byte 0
coordsX: .word 0,0,0,0,0,0,0,0
coordsY: .word 0,0,0,0,0,0,0,0
vx:      .word float2fix(1.5),float2fix(1.51),float2fix(1.52),float2fix(1.53)
         .word float2fix(1.54),float2fix(1.55),float2fix(1.56),float2fix(1.57)
vy:      .word float2fix(1.5),float2fix(1.51),float2fix(1.52),float2fix(1.53)
         .word float2fix(1.54),float2fix(1.55),float2fix(1.56),float2fix(1.57)
acc:     .word float2fix(1.008)
product: .word 0,0
spriteX: .word 0,0,0,0,0,0,0,0
spriteY: .word 0,0,0,0,0,0,0,0
